* standalone assisted teleop (ros-navigation#2904)

* standalone assisted teleop

* added in action message

* code review

* moved to behavior server

* added assisted teleop bt node

* revert

* added bt node for assisted teleop

* lint fix

* added cancel assisted teleop node

* code review

* working

* cleanup

* updated feeback

* code review

* update compute velocity

* cleanup

* lint fixes

* cleanup

* test fix

* starting to add tests for assisted teleop

* fixed tests

* undo

* fixed test

* is_recovery

* adjust abort result based on recovery or not

* code review

* added preempt velocity

* working preempt assisted teleop test

* completed assisted teleop tests

* code review

* undo

* code review

* remove sleep

* topic rename

* missing comma

* added comma :(

* added comma

Co-authored-by: Steve Macenski <[email protected]>

* Add the support of range sensors to Collision Monitor (ros-navigation#3099)

* Support range sensors in Collision Monitor

* Adjust README.md

* Meet review fixes

* Fix ros-navigation#3152: Costmap extend did not include Y component (ros-navigation#3153)

* missing nodes added to nav2_tree_nodes.xml (ros-navigation#3155)

* Change deprecated ceres function (ros-navigation#3158)

* Change deprecated function

* Update smoother_cost_function.hpp

* remove camera_rgb_joint since child frame does not exist (ros-navigation#3162)

* bugfix (ros-navigation#3109) deadlock when costmap receives new map (ros-navigation#3145)

* bugfix (ros-navigation#3109) deadlock when costmap receives new map

Signed-off-by: Daisuke Sato <[email protected]>

* introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts

Signed-off-by: Daisuke Sato <[email protected]>

Signed-off-by: Daisuke Sato <[email protected]>

* simple command costmap api - first few functions (ros-navigation#3159)

* initial commit costmap_2d template

Signed-off-by: Stevedan Omodolor <[email protected]>

* finish task A and tested

* lint

* Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py

Co-authored-by: Steve Macenski <[email protected]>

* fix trailing underscores

Signed-off-by: Stevedan Omodolor <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

* Fix missing dependency on nav2_collision_monitor (ros-navigation#3175)

* fixed start (ros-navigation#3168)

* fixed start

* return true

* fix tests

* Fix velocities comparison for rotation at place case (ros-navigation#3177)

* Fix velocities comparison for rotation at place case

* Meet review item

* Remove unnecessary header

* Change the comment

* set a empty path on halt (ros-navigation#3178)

* set a empty path on halt

* fixed issues

* remove path reset

* fixing

* reverting

* revert

* revert

* fixed lint

* test fix

* uncrusify fix

* simple command costmap api - update few functions (ros-navigation#3169)

* * add aditional function to costmap_2d.py

Signed-off-by: Stevedan Omodolor <[email protected]>

Updated-by: Jaehun Kim <[email protected]>

* finish task B

* Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py

* Update method docs

* Remove underscores at parameters and split getCost into getCostXY and getCostIdx

* Update method docstrings

* lint code & update docstring, remove default value of getCostXY

* lint code with pep257 & flake8

* clear names for bt nodes (ros-navigation#3183)

* [Smac] check if a node exists before creating (ros-navigation#3195)

* check if a node exists before creating

* invert logic to group like with like

* Update a_star.cpp

* fixing benchmarkign for planners (ros-navigation#3202)

* [Smac] Robin hood data structure improves performance by 10-15%! (ros-navigation#3201)

* adding robin_hood unordered_map

* using robin_hood node map

* ignore robin_hood file

* linting

* linting cont. for triple pointers

* linting cont. for uncrustify

* [RPP] Add parameter to enable/disable collision detection (ros-navigation#3204)

* [RPP] Add parameter to enable/disable collision detection

* [RPP] Update README

* Update waffle.model

* add benchmark launch file + instructions (ros-navigation#3218)

* removing hypotf from smac planner heuristic computation (ros-navigation#3217)

* removing hypotf

* swapping to node2d sqrt

* complete smac planner tolerances (ros-navigation#3219)

* Disable Output Buffering (ros-navigation#3220)

To ensure await asyncio prints [Processing: %s]' every 30s as expected

* fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (ros-navigation#3223)

* fix majority of python linting errors

* finish linting

* Assisted teleop simple commander (ros-navigation#3198)

* add assisted teleop to python api

* cleanup

* assisted teleop demo

* rename

* lint

* code review

* trigger build

* flake8 fix

* break cashe

* moved all v11 to v12

* lint fix

* remove package dep

* change default time allowance

* Costmap Filter enabling service (ros-navigation#3229)

* Add enabling service to costmap filters

* Add service testcase

* Fix comment

* Use toggle_filter service name

* Add binary flip costmap filter (ros-navigation#3228)

* Add binary flip costmap filter

* Move transformPose, worldToMask, getMaskData to CostmapFilter

* Added default parametrized binary filter state

* Switched to std_msgs/msg/Bool.msg

* Use arbitrary filter values

* Update waffle.model

* Update waffle.model

* Update test_actions.cpp

* odom alpha restriction to avoid overflow caused by user-misconfiguration (ros-navigation#3238)

* odom alpha restriction

* odom alpha code style

* odom alpha code style

* odom alpha code style

* Update controller server goal checker (ros-navigation#3240)

* [FIX] Update controller server goal checker

* [FIX] Autoformat code

* [FIX] Misplaced tabs.

Co-authored-by: Pedro Alejandro González <[email protected]>

* map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (ros-navigation#3242)

* odom alpha restriction

* odom alpha code style

* odom alpha code style

* odom alpha code style

* map-size restriction

* map-size code style

* map-size rejection

* map-size codestyle

* map-size return false

* Add Path Smoothers Benchmarking suite (ros-navigation#3236)

* Add Path Smoothers Benchmarking suite

* Meet review items

* Update tools/smoother_benchmarking/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Move optional performance patch to the end of README

* Fix README

Co-authored-by: Steve Macenski <[email protected]>

* Fix typo (ros-navigation#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* Added Line Iterator (ros-navigation#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <[email protected]>

* bumping to 1.1.3 for release

Signed-off-by: Daisuke Sato <[email protected]>
Signed-off-by: Stevedan Omodolor <[email protected]>
Co-authored-by: Joshua Wallace <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: Abdullah Enes BEDİR <[email protected]>
Co-authored-by: Tobias Fischer <[email protected]>
Co-authored-by: Tejas Kumar Shastha <[email protected]>
Co-authored-by: Daisuke Sato <[email protected]>
Co-authored-by: Stevedan Ogochukwu Omodolor <[email protected]>
Co-authored-by: Lukas Fanta <[email protected]>
Co-authored-by: Jackson9 <[email protected]>
Co-authored-by: Ruffin <[email protected]>
Co-authored-by: Hao-Xuan Song <[email protected]>
Co-authored-by: Nicolas Rocha Pacheco <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>
Co-authored-by: jaeminSHIN <[email protected]>
Co-authored-by: Afif Swaidan <[email protected]>
Co-authored-by: Afif Swaidan <[email protected]>

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)

* BT Service Node to throw if service was not available in time (ros-navigation#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* Remove duplicate of nav2_back_up_cancel_bt_node (ros-navigation#3332)

* Remove unused velocity scaling config from example xml (ros-navigation#3330)

Signed-off-by: Borong Yuan <[email protected]>

Signed-off-by: Borong Yuan <[email protected]>

* Make mapUpdateLoop() indicator variables to be thread-safe (ros-navigation#3308)

* Ensure that plugin initialization to be called before updating routines (ros-navigation#3307)

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <[email protected]>

* updated copyright in modified file

Signed-off-by: Owen Hooper <[email protected]>

Signed-off-by: Owen Hooper <[email protected]>

* Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* bump to 1.1.3 for humble sync 3

Signed-off-by: Borong Yuan <[email protected]>
Signed-off-by: Owen Hooper <[email protected]>
Co-authored-by: Adam Aposhian <[email protected]>
Co-authored-by: Erwin Lejeune <[email protected]>
Co-authored-by: Sven Langner <[email protected]>
Co-authored-by: Borong Yuan <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: MartiBolet <[email protected]>
Co-authored-by: Owen Hooper <[email protected]>
Co-authored-by: Pedro Alejandro González <[email protected]>

…on#3337)

* Revert "remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)" (ros-navigation#3336)

This reverts commit 90ae5cc.

* bump to 1.1.5 for release

* Std::string doesn't have such constructor, therefore wrong exception happens when waiting for action server is over (ros-navigation#3333)

* Update bt_action_node.hpp

* Update bt_cancel_action_node.hpp

* Update bt_action_node.hpp

* Update bt_cancel_action_node.hpp

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap (ros-navigation#3356)

* Do only consider enabled costmap plugins/filters in the isCurrent() method of a LayeredCostmap

When clearing entirely a costmap (see the `clear_entirely_*` service),
its layers are reset. This sets their `current_` attribute to False,
until they are updated (`updateCosts()`). However, for disabled
plugins/filters, the `updateCosts()` is bypassed so that the `current_`
attribute is never reset to True, and the costmap never becomes
"current" again; because of that the controller_server used to get stuck
in an endless loop (see `while (!costmap_ros_->isCurrent())`
in `ControllerServer::computeControl()`).

This patch fixes that by adding a condition for not considering disabled
plugins/filters in the `LayeredCostmap::isCurrent()`.

* Add forgotten Layer::isEnabled() definition

* Export ompl include directories from nav2_smac_planner. (ros-navigation#3358)

* Update default w_smooth to 2M (ros-navigation#3341)

* Fix nav2_velocity_smoother for sim time (ros-navigation#3360)

* Fix nav2_velocity_smoother for sim time

* Update nav2_velocity_smoother/src/velocity_smoother.cpp

Co-authored-by: Steve Macenski <[email protected]>

* catch general exceptions (ros-navigation#3373)

* catch general exceptions

* Update controller_server.cpp

* nav2_tree_nodes: rename "service_name" input port for action nodes (ros-navigation#3370)

Classes inheriting from nav2_behavior_tree::BtActionNode use
"server_name" as input port. The "service_name" input port is reserved
to sub-classes of nav2_behavior_tree::BtServiceNode. There seemed to be
a confusion in the nav2_tree_nodes.xml file.

* Fixing Theta* test failures (ros-navigation#3380)

* adding printouts since can't see locally

* one more to isolate

* request in bounds

* remove prints

* Fix SpeedController (ros-navigation#3378)

* Fix SpeedController
* Using blackboard get() method that does not throw error
* Share odom_smoother on the blackboard

Signed-off-by: Borong Yuan <[email protected]>

* Fix SpeedController linting and test errors

Signed-off-by: Borong Yuan <[email protected]>

Signed-off-by: Borong Yuan <[email protected]>

* node_name change during initialization (ros-navigation#3387)

* Added missing conditions to nav2_tree_nodes (ros-navigation#3401)

* Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp to nav2_tree_nodes

* Add WouldAControllerRecoveryHelp and WouldAPlannerRecoveryHelp...... to nav2_tree_nodes

* Update nav2_tree_nodes.xml

---------

Co-authored-by: blanker <[email protected]>

* Fix multi-robot launch issues (ros-navigation#3383)

* Fix multi-robot launch issues:
* Use full name of composable nodes container
* Correct and update BT plugins and error codes list
* Fix and update local and global costmap used plugins parameters

* Do not publish voxel map for multi-robot case

* Update controller_server.cpp

* Update nav2_smoother.cpp

---------

Signed-off-by: Borong Yuan <[email protected]>
Co-authored-by: Andrii Maistruk <[email protected]>
Co-authored-by: milidam <[email protected]>
Co-authored-by: Marc Alban <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: HovorunB <[email protected]>
Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com>
Co-authored-by: Borong Yuan <[email protected]>
Co-authored-by: Austin Greisman <[email protected]>
Co-authored-by: muttistefano <[email protected]>
Co-authored-by: blanker <[email protected]>